#ifndef INTERPOLATION_H
#define INTERPOLATION_H

#include <tbb\concurrent_vector.h>
#include <tbb\blocked_range2d.h>
#include <tbb\parallel_for.h>

#include <boost/shared_ptr.hpp>
#include <boost/lexical_cast.hpp>

#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_linalg.h>

#include <math.h>
#include <iostream>

#include "Raster.hpp"
#include "Logger.hpp"
namespace Borealis
{
	namespace Interpolation
	{

	/*
	Class: ApplyIDW
	Applies an IDW from the points to the raster
	Not to be called directly
	*/
	class ApplyIDW
	{
		boost::shared_ptr< Raster<float> > const m_raster;
		tbb::concurrent_vector< RasterPoint<float> >* const m_points;
		RasterVisitor<float>* const m_visitor;
	public:
		/*
		Function: ApplyIDW
		Applies an IDW scheme. Optionally applies a visitor after the cell's IDW value is calculated.

		Parameters: 
		Raster<float> * inRaster - Raster to apply IDW to
		tbb::concurrent_vector<RasterPoint<float> > * inPoints - Points to use in IDS
		RasterVisitor * v - Visitor to apply. 

		Throws:
		Never

		Returns:   
		- 
		*/
		ApplyIDW(boost::shared_ptr<Raster<float> > inRaster, tbb::concurrent_vector<RasterPoint<float> >* inPoints, RasterVisitor<float>* v=NULL) 
			: m_raster(inRaster), m_points(inPoints), m_visitor(v) 
		{

		}

		void operator()(const tbb::blocked_range2d<unsigned int>& r) const
		{
			boost::shared_ptr<Raster<float> > raster = m_raster;
			tbb::concurrent_vector<RasterPoint<float> >* points = m_points;
			RasterVisitor<float>* visitor = m_visitor;

			float eps = 0.001F;

			for(unsigned int row = r.rows().begin(); row!=r.rows().end();++row)
			{
				for(unsigned int col = r.cols().begin(); col!=r.cols().end(); ++col)
				{

					double numerator = 0.0;
					double denominator = 0.0;

					float z0 = -9999.0F;
					bool xyIsStation=false; //currently x,y is a station location
					double valueOfStation= -9999.0;

					for(unsigned int k = 0; k < points->size(); k++)
					{
						double xdiff = (double)col-(double)points->at(k).col;
						double ydiff = (double)row-(double)points->at(k).row;
						double di = pow(
										sqrt(
												pow(xdiff,2.0) + pow(ydiff,2.0)
											)
										,2.0
									   );
						if(di <= 0)
						{
							numerator = points->at(k).z;
							denominator = 1.0;
							break;
						}
						else
						{
							numerator += points->at(k).z/ di;
							denominator += 1 / di; 

						}

					}

					z0 = (float)(numerator/denominator);

					if(visitor)
					{
						visitor->operator ()(&z0,row,col);
					}

					raster->SetRasterAt(row,col,z0);
				}
			}

		}
	};

	//no to be called direction, but through
	//ThinPlateSplineT(...)
	class ApplyTPST
	{
		boost::shared_ptr< Raster<float> > const m_raster;
		tbb::concurrent_vector< RasterPoint<float> >* const m_points;
		RasterVisitor<float>* const m_visitor;


	public:
		/*
		Function: ApplyTPST
		Applies an TPST scheme. Optionally applies a visitor after the cell's IDW value is calculated.

		Parameters: 
		Raster<float> * inRaster - Raster to apply TPST to
		tbb::concurrent_vector<RasterPoint<float> > * inPoints - Points to use in IDS
		RasterVisitor * v - Visitor to apply. 

		Throws:
		Never

		Returns:   
		- 
		*/
		ApplyTPST(boost::shared_ptr<Raster<float> > inRaster, tbb::concurrent_vector<RasterPoint<float> >* inPoints, RasterVisitor<float>* v=NULL) 
			: m_raster(inRaster), m_points(inPoints), m_visitor(v) 
		{

		}

		void operator()(const tbb::blocked_range2d<unsigned int>& r) const
		{
			boost::shared_ptr<Raster<float> > raster = m_raster;
			tbb::concurrent_vector<RasterPoint<float> >* points = m_points;
			RasterVisitor<float>* visitor = m_visitor;


			for(unsigned int row = r.rows().begin(); row!=r.rows().end();++row)
			{
				for(unsigned int col = r.cols().begin(); col!=r.cols().end(); ++col)
				{
					unsigned int size = points->size();
					size++; // need to make room for the constants

					double *A = new double[ size * size ];
					double pi = 3.14159;
					double c = 0.577215; //euler constant
					double *B = new double[size]; // known values - constant value of 0 goes in b[size-1]

					for(unsigned int i=0;i < size-1; i++)
					{
						for(unsigned int j=i; j < size-1; j++)
						{
							double dij = sqrt( pow((int)(points->at(i).col) - (int)(points->at(j).col),2.0) + pow((int)(points->at(i).row) - (int)(points->at(j).row),2.0) );
							double Rd = 0.0;

							if ( (dij - 0.0) < 0.000001)
								Rd = 0.0;
							else
								Rd = -1.0/(2.0*pi*0.01)*( log(dij*0.1/2.0) + c + gsl_sf_bessel_K0(dij*0.1));

							//A(i,j+1) = Rd;
							A[i * size + (j+1)] = Rd;

							//A(j,i+1) = Rd;
							A[j * size + (i+1)] = Rd;			
						}
					}

					//set constants and build b values
					for(unsigned int i = 0; i< size;i++)
					{
						A[i*size+0] = 1;
						A[(size-1)*size+i] = 1;
					}

					for(unsigned int i = 0; i<size -1; i++)
					{
						B[i] = points->at(i).z;
					}
					B[size-1] = 0.0; //constant


					A[(size-1)*(size)] = 0.0;

					//solve equation via gsl via lu decomp
					gsl_matrix_view m 	= gsl_matrix_view_array (A, size, size);
					gsl_vector_view b	= gsl_vector_view_array (B, size);
					gsl_vector *x		= gsl_vector_alloc (size);
					int s				= 0;
					gsl_permutation * p = gsl_permutation_alloc (size);
					gsl_linalg_LU_decomp (&m.matrix, p, &s);
					gsl_linalg_LU_solve (&m.matrix, p, &b.vector, x);

					//location of the unknown
					unsigned int ux = col;
					unsigned int uy = row;

					double z0 = x->data[0];//little a

					//-1 to skip the x[0] we already pulled off above
					for (unsigned int i = 0;i < (x->size - 1) ;i++)
					{
						double dxy = sqrt( pow((int)(ux) - (int)(points->at(i).col),2.0) + pow((int)(uy) - (int)(points->at(i).row),2.0) );
						double Rd = 0.0;

						if ((dxy - 0.0) < 0.000001)
							Rd = 0.0;
						else
							Rd = -1.0/(2.0*pi*0.01)*( log(dxy*0.1/2.0) + c + gsl_sf_bessel_K0(dxy*0.1));

						z0 = z0 + x->data[i+1]*Rd; 
					}


					gsl_permutation_free (p);
					gsl_vector_free (x);
					delete B;
					delete A;

					//loss of precision
					float fz0 = (float)z0;

					if(visitor)
					{
						visitor->operator ()(&fz0,row,col);
					}

					raster->SetRasterAt(row,col,fz0);
				}
					
			}
		}

		
	};
	
	/*
	Function: InverseDistanceWeighting
		Applies IDW to the selected points

	Parameters: 
		tbb::concurrent_vector<RasterPoint<float> > & points - x,y,z reference points
		Raster<float> * raster - Raster to save IDW results to
		RasterVisitor<float> * visitor - Optional visitor to apply once IDW value has been calculated for a cell value

	Throws:
		std::runtime_exception on error

	Returns:   
		void - 
	*/
	void InverseDistanceWeighting(tbb::concurrent_vector<RasterPoint<float> >& points,boost::shared_ptr<Raster<float> > raster, RasterVisitor<float>* visitor = NULL);

	/*
		Function: ThinPlateSplineT
			 Implements thin plated splines with tension. Has a default tension value of 0.1
		
		Parameters: 
			tbb::concurrent_vector<RasterPoint<float> > & points - points to interpolate from
			boost::shared_ptr<Raster<float> > raster - Raster domain to interpolate over
			RasterVisitor<float> * visitor - Optional visitor to apply to a cell 
			double tension - Tension to apply. default value of 0.1
		
		Throws:
			std::runtime_exception on error
		
		Returns:   
			void - 
	*/
	void ThinPlateSplineT(tbb::concurrent_vector<RasterPoint<float> >& points,boost::shared_ptr<Raster<float> > raster, RasterVisitor<float>* visitor = NULL, double tension = 0.1);



}
}
#endif